//This function computes the projection onto a simplex.
//Input data of the type N*M1*M2*M3...
//Project the first dimension.
//tc. 2011.12.21

#include "mex.h"
#include <string.h>

void qsort(double *data,int a,int b){
	int x,y;
	double flag = data[(a+b)/2];
	double temp;
	x = a;
	y = b;
	while (x<=y){
		while ((data[x]<flag)&&(x<=y)) x++;
		while ((data[y]>flag)&&(x<=y)) y--;
		if (x<=y){
			temp = data[x];
			data[x] = data[y];
			data[y] = temp;
			x++;
			y--;
		}
	}
	if (x<b) qsort(data,x,b);
	if (y>a) qsort(data,a,y);
}

double* proj(const double* data, int n)
{
	double* z = new double [n];
	memcpy(z, data, sizeof(double)*n);
	qsort(z,0,n-1);
	double* p = new double [n];
    memset(p, 0, sizeof(double)*n);
	double s = -1;
	double t = 0;
	bool flag = true;
	for (int i = n-1; i > 0; i--)
	{
		s += z[i];
		t = s / (n - i);
		if ( t > z[i-1] ) 
		{
			flag = false;
			break;
		}
	}
	if (flag == true)
	{
		s += z[0];
		t = s / n;
	}
	for (int j = 0; j < n; j++)
	{
		p[j] = data[j] > t ? data[j] - t : 0;
	}
	return p;
}
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, mxArray* prhs[])
{
	if ( nrhs == 0) 
		mexErrMsgTxt("One input argument is required.");
	else if( nrhs > 1)
		mexErrMsgTxt("Too many input arguments.");
	else if ( nlhs > 1)
		mexErrMsgTxt("Too many ouput arguments.");
	else if ( !mxIsNumeric(prhs[0]) )
		mexErrMsgTxt("Input must be numeric.");
	else if ( mxIsComplex(prhs[0]) )
		mexErrMsgTxt("Input must be real numbers.");
	
	int m = mxGetM(prhs[0]); 
	int n = mxGetN(prhs[0]);
	double* p = mxGetPr(prhs[0]);
	plhs[0] = mxCreateNumericArray(mxGetNumberOfDimensions(prhs[0]), 
					mxGetDimensions(prhs[0]),mxDOUBLE_CLASS, mxREAL);
	double* c = mxGetPr(plhs[0]);
	for (int j = 0; j < n; j++)
	{
		double* tmp = proj(p+j*m,m);
		memcpy(c+j*m, tmp, sizeof(double)*m);
	}
}